#include "robot_manager.h"
#include "utilities_function.h"

void RobotManager::publishJointStatesLoop()
{
    rclcpp::Rate rate(CONTROL_FREQ);
    std::vector<double> q(7, 0.0), qd(7, 0.0), qdd(7, 0.0), torque;
    std::vector<double> D = {1.0, 1.2, 1.1, 1.1, 1.3, 1.3, 1.0};

    std::vector<double> collision_force_threshold;
    this->get_parameter("collision_force_threshold", collision_force_threshold);
    while (running_ && rclcpp::ok())
    {
        if (!is_simulation_)
        {
            pthread_mutex_lock(&zoros_mutex_);
            joint_state_msg_.header.stamp = this->now();

            for (int i = 0; i < 7; i++)
            {
                double position = motors_[i].Get_Position();
                if (position > 3 || position < -3)
                {
                    static unsigned int j;
                    if (j == 100)
                    {
                        ZLOGW("joint [%d] position: %.4f out of range!", i + 1, position);
                        j = 0;
                    }
                    j++;
                }
                dm_->refresh_motor_status(motors_[i]);
                joint_state_msg_.position[i] = position;
                joint_state_msg_.velocity[i] = motors_[i].Get_Velocity();
                joint_state_msg_.effort[i] = motors_[i].Get_tau();
            }
            
            // 处理gripper控制的joint8和joint9
            // gripper_电机角度范围[0, 5.4]rad对应joint8/joint9各自0-46mm的联动
            dm_->refresh_motor_status(gripper_);
            double gripper_angle = gripper_.Get_Position();  // 获取gripper电机角度
            
            // 将gripper电机角度[0, 5.4]rad映射到joint8位置[0, 0.046]m
            constexpr double kMaxGripperAngle = 5.4; // rad
            constexpr double kMaxJointPos = 0.046;   // m (46mm)

            // 映射到 joint8 位置（0-46mm），并对输入角度进行饱和
            const double joint8_pos = LinearMap(gripper_angle,
                                                0.0, kMaxGripperAngle,
                                                0.0, kMaxJointPos,
                                                /*saturate_input=*/true);
            
            joint_state_msg_.position[7] = joint8_pos;      // joint8
            joint_state_msg_.position[8] = -joint8_pos;     // joint9 (mimic with multiplier=-1)
            const double vel_scale = (kMaxJointPos / kMaxGripperAngle);
            joint_state_msg_.velocity[7] = gripper_.Get_Velocity() * vel_scale;
            joint_state_msg_.velocity[8] = -gripper_.Get_Velocity() * vel_scale;
            joint_state_msg_.effort[7] = gripper_.Get_tau();
            joint_state_msg_.effort[8] = gripper_.Get_tau();
            
            static unsigned int j;
            if (j == 100)
            {
                ZLOGI("position: %f, %f, %f, %f, %f, %f, %f, %f, %f", joint_state_msg_.position[0], joint_state_msg_.position[1],
                    joint_state_msg_.position[2], joint_state_msg_.position[3], joint_state_msg_.position[4],
                    joint_state_msg_.position[5], joint_state_msg_.position[6], joint_state_msg_.position[7], joint_state_msg_.position[8]);
                j = 0;
            }
            j++;   
#if 0
//! TODO: 碰撞检测逻辑还有问题
            q = joint_state_msg_.position;
            robot_model_->inverseDynamics(q, qd, qdd, torque);
            if(teach_mode_ == false)
            {
                for(int i = 0; i < 7; i++)
                {
                    if(fabs(joint_state_msg_.velocity[i]) > 0.02 && fabs(torque[i] * D[i] - joint_state_msg_.effort[i]) > collision_force_threshold[i])
                    {
                        ZLOGW("Joint [%d] torque over limit, maybe collision!", i + 1);
                        teach_mode_ = true;
                        break;
                    }
                }
            }
            else
            {
                for(int i = 0; i < 7; i++)
                {
                    dm_->control_mit(motors_[i], 0.0, 0.0, 0.0, 0.0, torque[i] * D[i]);
                }
            }
#endif
            joint_pub_->publish(joint_state_msg_);
            pthread_mutex_unlock(&zoros_mutex_);
        }
        else
        {
            // 仿真模式：发布当前的joint_state_msg_
            pthread_mutex_lock(&zoros_mutex_);
            joint_state_msg_.header.stamp = this->now();
            joint_pub_->publish(joint_state_msg_);
            pthread_mutex_unlock(&zoros_mutex_);
        }

        rate.sleep();
    }
}